package gnss

import (
	"bytes"
	"dgo/goutils/utils"
	"fmt"
	"math"
	"runtime"
	"sort"
	"sync/atomic"
	"time"
)

const (
	MAX_SIGN_LEN = 32
)

/*
获取波长
*/
type Func_GetWl func(navisys byte, satid uint16, sig byte) float64

type Func_GetFreq func(navisys byte, satid uint16, sig byte) float64

/*
*

	卫星的观测值
*/
type RtcmRawObsSat struct {
	Satid   byte // 卫星号
	MsmType byte
	RoughRR float64 //

	// DF399 GNSS 卫星低精度的载波 相位变化速率（rough phase range rate）  范围 ±8191m/s 比例因子1m/s
	RoughPRR int16 // ±8191 m/s
	BitFlag  byte

	//PsRngV map[byte]float64 // 伪距  信号对应的PsRng值, 0~31, 0(idx): 1(sigid)  (idx+1=sigid)
	PsRngV [MAX_SIGN_LEN]float64 // 伪距  高精度伪距 信号对应的PsRng值, 0~31, 0(idx): 1(sigid)  (idx+1=sigid)
	PhRngV [MAX_SIGN_LEN]float64 // 相位  信号高精度载波相位

	/**
	DF404 GNSS 信号高精度载波相 位变化速率（fine PhaseRate） 范围±1.6384m/s 比例因子：0.0001m/s
	 Fine Phaserange Rate for a given signal. Full Phaserange Rate is the
	sum of this field and the Satellite Rough Phaserange Rate (DF399).
	A bit pattern equivalent to 4000h (–1.6384 m/s) indicates invalid value
	*/
	PhaRR [MAX_SIGN_LEN]int16 // GNSS signal fine Phaserange Rate

	/*
		0 – No half-cycle ambiguity.
		1 – Half-cycle ambiguity
	*/
	LkTV    [MAX_SIGN_LEN]uint16
	HalfAmb [MAX_SIGN_LEN]byte
	Cnr     [MAX_SIGN_LEN]float32
	LLI     [MAX_SIGN_LEN]byte
}

func NewRtcmRawObsSat() *RtcmRawObsSat {
	rval := &RtcmRawObsSat{}
	atomic.AddInt32(&rtcm_raw_sat_obs_cnt, 1)
	runtime.SetFinalizer(rval, func(obj interface{}) {
		atomic.AddInt32(&rtcm_raw_sat_obs_cnt, -1)
	})
	return rval
}

func (this *RtcmRawObsSat) ExtractSigs() []byte {
	var rval [MAX_SIGN_LEN]byte
	j := 0

	for i := U8_0; i < MAX_SIGN_LEN; i++ {
		if this.IsValid(i) {
			rval[j] = byte(i + 1)
			j++
		}
	}

	return rval[:j]
}

func (this *RtcmRawObsSat) Reset() {
	this.Satid = 0
	this.RoughRR = 0
	this.RoughPRR = 0
	this.BitFlag = 0
	ResetSigListFloat64(&this.PsRngV)
	ResetSigListFloat64(&this.PhRngV)
	ResetSigListInt16(&this.PhaRR)
	ResetSigListUint16(&this.LkTV)
	ResetSigListByte(&this.HalfAmb)
	ResetSigListFloat32(&this.Cnr)

	//this.PsRngV = [MAX_SIGN_LEN]float64{0}
	//this.PhRngV = [MAX_SIGN_LEN]float64{0}
	//this.PhaRR = [MAX_SIGN_LEN]int16{0}
	//this.LkTV = [MAX_SIGN_LEN]uint16{0}
	//this.HalfAmb = [MAX_SIGN_LEN]byte{0}
	//this.Cnr = [MAX_SIGN_LEN]float32{0}
}

func (this *RtcmRawObsSat) IsValidSignal(sigId byte) bool {
	if sigId == 0 {
		return false
	}
	return this.IsValid(sigId - 1)
}

func (this *RtcmRawObsSat) IsValid(idx byte) bool {
	return this.PsRngV[idx] != 0 || this.PhRngV[idx] != 0 || this.Cnr[idx] != 0
	//return (this.PsRngV[idx] != 0 && this.PsRngV[idx] != PS_INVALID_VALUE) || (this.PhRngV[idx] != 0 && this.PhRngV[idx] != PH_INVALID_VALUE)
	//return (this.PsRngV[idx] != 0 && this.PsRngV[idx] != PS_INVALID_VALUE) && (this.PhRngV[idx] != 0 && this.PhRngV[idx] != PH_INVALID_VALUE)
}

func (this *RtcmRawObsSat) FillSignMask(m uint32) uint32 {
	for i := U8_0; i < 32; i++ {
		if this.IsValid(i) {
			m |= (1 << (31 - i))
		}
	}
	return m
}

func (this *RtcmRawObsSat) SetLockTV(v uint16) {
	for i := U8_0; i < 32; i++ {
		if this.IsValid(i) {
			this.LkTV[i] = v
		}
	}
}

func (this *RtcmRawObsSat) PrintRaw(naviSys byte, bb *utils.BytesBuilder) {
	if naviSys == 0 {
		return
	}
	bb.AppendStr(GetPrnStrCode(this.Satid, naviSys)).AppendStr("\t")
	bb.Appendf("RoughR %s\t", RinexFormatFloat(this.RoughRR))
	for i := U8_0; i < 32; i++ {
		if this.IsValid(i) {
			bb.Appendf("%s\t", GetSignCode(i+1, naviSys))
			bb.AppendFixedStr(fmt.Sprintf("%.4f", this.PsRngV[i]), 14, " ")
			bb.AppendFixedStr(fmt.Sprintf("%.4f", this.PhRngV[i]), 14, " ")
			bb.AppendFixedStr(fmt.Sprintf("%d", this.LkTV[i]), 10, " ")
			bb.AppendFixedStr(fmt.Sprintf("%d", this.HalfAmb[i]), 10, " ")
			bb.AppendFixedStr(fmt.Sprintf("%.2f", this.Cnr[i]), 10, "")
		}
	}
	bb.AppendStr("\n")
}

func (this *RtcmRawObsSat) Print(naviSys byte, bb *utils.BytesBuilder) {
	if naviSys == 0 {
		return
	}
	bb.AppendStr(GetPrnStrCode(this.Satid, naviSys)).AppendStr("\t")
	bb.Appendf("RoughR %s\t", RinexFormatFloat(this.RoughRR))
	for i := U8_0; i < 32; i++ {
		if this.IsValid(i) {
			bb.Appendf("%s\t", GetSignCode(i+1, naviSys))
			bb.WriteString(RinexFormatFloat(this.GetPsPng(i + 1)))
			bb.WriteString(RinexFormatFloat(this.GetPhPng(naviSys, i+1)))
			bb.AppendFixedStr(fmt.Sprintf("%d", this.LkTV[i]), 10, " ")
			bb.AppendFixedStr(fmt.Sprintf("%d", this.HalfAmb[i]), 10, " ")
			bb.AppendFixedStr(fmt.Sprintf("%.2f", this.Cnr[i]), 10, "")
		}
	}
	bb.AppendStr("\n")
}

func (this *RtcmRawObsSat) GetObsSign(naviSys byte, idx byte) *GnssObsSign {
	rval := &GnssObsSign{}
	this.WriteObsSign(naviSys, idx, rval)
	return rval
}

func (this *RtcmRawObsSat) WriteObsSign(naviSys byte, idx byte, outObsSign *GnssObsSign) {
	outObsSign.SatId = this.Satid
	outObsSign.SigId = SigiRinexCode(naviSys, idx+1)
	outObsSign.NaviSys = naviSys
	outObsSign.PsRng = this.GetPsPng(idx + 1)
	outObsSign.PhRng = this.GetPhPng(naviSys, idx+1)
	outObsSign.LkT = this.LkTV[idx]
	outObsSign.HalfAmb = this.HalfAmb[idx]
	outObsSign.Cnr = this.Cnr[idx]
}

func (this *RtcmRawObsSat) Compare(naviSys byte, slave *RtcmRawObsSat, fn func(obs_m, obs_s *GnssObsSign) bool) (rval bool) {
	var obs0, obs1 GnssObsSign
	for i := U8_0; i < 32; i++ {
		if this.IsValid(i) {
			this.WriteObsSign(naviSys, i, &obs0)
			slave.WriteObsSign(naviSys, i, &obs1)
			rval = fn(&obs0, &obs1)
			if !rval {
				break
			}
		}
	}
	return
}

func (this *RtcmRawObsSat) PrintWithRaw(naviSys byte, bb *utils.BytesBuilder) {
	if naviSys == 0 {
		return
	}
	bb.AppendStr(GetPrnStrCode(this.Satid, naviSys)).AppendStr("\t")
	bb.AppendStr(" RR ")
	bb.AppendStrPrefixForWidth(fmt.Sprintf("%.4f ", this.RoughRR), 14, " ")
	bb.AppendStr(" PRR ")
	bb.AppendStrPrefixForWidth(fmt.Sprintf("%d ", this.RoughPRR), 6, " ")

	for i := U8_0; i < 32; i++ {
		if this.IsValid(i) {
			bb.Appendf("%s(%.2d)\t", GetSignCode(i+1, naviSys), i+1)
			bb.WriteString(RinexFormatFloat(this.GetPsPng(i + 1)))
			bb.AppendFixedStr(fmt.Sprintf("(%.4f)", this.PsRngV[i]), 14, " ")
			bb.WriteString(RinexFormatFloat(this.GetPhPng(naviSys, i+1)))
			bb.AppendFixedStr(fmt.Sprintf("(%.4f)", this.PhRngV[i]), 14, " ")
			bb.AppendFixedStr(fmt.Sprintf("%d ", this.LkTV[i]), 10, " ")
			bb.AppendFixedStr(fmt.Sprintf("%d ", this.HalfAmb[i]), 5, " ")
			bb.AppendFixedStr(fmt.Sprintf("%.4f ", this.Cnr[i]), 5, " ")
			bb.AppendStrPrefixForWidth(fmt.Sprintf("%.6f ", this.GetPhRR(naviSys, i+1)), 14, " ")
			bb.AppendStrPrefixForWidth(fmt.Sprintf("(%d) ", this.PhaRR[i]), 8, " ")
			bb.AppendStrPrefixForWidth(fmt.Sprintf("%.4f ", this.GetDop(naviSys, i+1)), 14, " ")
		}
	}
	bb.AppendStr("\n")
}

func (this *RtcmRawObsSat) SetPsPng(sigId byte, v float64) {
	if this.RoughRR == 0 {
		this.RoughRR = math.Round(v/P2_10_RANGE_MS) * P2_10_RANGE_MS
	}
	if v == 0 {
		this.PsRngV[sigId-1] = PS_INVALID_VALUE
		return
	} else {
		v0 := v - this.RoughRR
		if math.Abs(v0) > 293 {
			v0 = PS_INVALID_VALUE
			//panic(fmt.Sprintf("%.4f out of psrng(Approx: ±292 m) ref: RTCM DF405", v0))
		}
		//if v0 == 0 {
		//	v0 = PS_ZERO_VALUE
		//}
		this.PsRngV[sigId-1] = v0
	}
}

func (this *RtcmRawObsSat) EmptySigValues(sigId byte) {
	this.PsRngV[sigId-1] = 0
	this.PhRngV[sigId-1] = 0
	this.PhaRR[sigId-1] = 0
	this.Cnr[sigId-1] = 0
	this.HalfAmb[sigId-1] = 0
	this.LkTV[sigId-1] = 0
}

func (this *RtcmRawObsSat) SetPhRR(naviSys byte, sigId byte, v float64) {
	v0 := v - float64(this.RoughPRR)
	this.PhaRR[sigId-1] = int16(math.Round(v0 / 1e-4))
}

/*
单位 m/s
*/
func (this *RtcmRawObsSat) GetPhRR(naviSys byte, sigId byte) float64 {
	prr := this.PhaRR[sigId-1]
	if prr == PS_INVALID_VALUE {
		return 0
	}
	rval := float64(float64(this.RoughPRR) + float64(prr)*1e-4)
	return rval
}

func (this *RtcmRawObsSat) SetDop(naviSys byte, sigId byte, v float64) {
	v0 := v - float64(this.RoughPRR)
	this.PhaRR[sigId-1] = int16(math.Round(v0 / 1e-4))
}

/*
if (rr&&rrf&&rrf[j]>-1E12) {
rtcm->obs.data[index].D[idx[k]]=(float)(-(rr[i]+rrf[j])*freq/CLIGHT);
}
*/
func (this *RtcmRawObsSat) GetDop(naviSys byte, sigId byte) float64 {
	prr := this.PhaRR[sigId-1]
	if prr == PS_INVALID_VALUE {
		return 0
	}
	rval := float64(float64(this.RoughPRR) + float64(prr)*1e-4)
	return rval
}

/*
v : unit is cycles, not meters
*/
func (this *RtcmRawObsSat) SetPhPngEx(naviSys byte, sigId byte, v float64, lli *byte, vars *RtcmObsVars, wl float64) {
	if wl == 0 {
		this.PhRngV[sigId-1] = PH_INVALID_VALUE
		return
		// panic(fmt.Sprintf("未知的波长[%s][%d]", GetPrnStrCode(this.Satid, naviSys), sigId))
	}
	var rngv float64 = 0
	if v == 0 {
		rngv = PH_INVALID_VALUE
		if vars != nil {
			cp := math.Round(rngv/wl) * wl
			vars.SetCp(naviSys, this.Satid, sigId, cp)
		}
	} else {
		m0 := v * wl
		rngv = m0 - this.RoughRR

		///* subtract phase - psudorange integer cycle offset */
		//LLI=data->LLI[j];
		//if ((LLI&1)||fabs(phrng_s-rtcm->cp[data->sat-1][j])>1171.0) {
		//	rtcm->cp[data->sat-1][j]=ROUND(phrng_s/lambda)*lambda;
		//	LLI|=1;
		//}
		//phrng_s-=rtcm->cp[data->sat-1][j];

		if vars != nil {
			//if this.Satid == 5 && naviSys == SYS_GPS && sigId == YOBS_GPS_SIGN_L2_2W {
			//	utils.Log.Debugf("hello");
			//}
			cp := vars.GetCp(naviSys, this.Satid, sigId)
			//if cp != 0{
			//	cp = cp;
			//}
			if *lli == 1 || math.Abs(rngv-cp) > 1171.0 {
				//if (naviSys == SYS_GLO && sigId == 2 && this.Satid == 1){
				//	utils.Log.Debug("debug");
				//}
				cp = math.Round(rngv/wl) * wl
				vars.SetCp(naviSys, this.Satid, sigId, cp)

				*lli = 1

				//this.Cp[sigId -1] = math.Round(rngv/wl) * wl;
			}
			rngv -= cp
		}
	}
	this.PhRngV[sigId-1] = rngv
}

/*
v : unit is cycles, not meters
*/
func (this *RtcmRawObsSat) SetPhPng(naviSys byte, sigId byte, v float64, lli *byte, vars *RtcmObsVars, getsigwl Func_GetWl) {
	var wl float64
	if getsigwl != nil {
		wl = getsigwl(naviSys, uint16(this.Satid), sigId)
	}
	if wl == 0 {
		wl = SigWaveLen(naviSys, sigId, this.Satid)
	}
	this.SetPhPngEx(naviSys, sigId, v, lli, vars, wl)
}

//func (this *RtcmRawObsSat) GetPsPngRaw(idx byte) float64 {
//	rval := this.PsRngV[idx]
//	//if rval == PS_ZERO_VALUE {
//	//	return 0
//	//}
//	return rval
//}

func (this *RtcmRawObsSat) GetPsPng(sigId byte) float64 {
	if this.PsRngV[sigId-1] == PS_INVALID_VALUE {
		return 0
		//} else if this.PsRngV[sigId-1] == PS_ZERO_VALUE {
		//	return this.RoughRR
	}
	return this.PsRngV[sigId-1] + this.RoughRR
}

func (this *RtcmRawObsSat) GetPhPng(naviSys byte, sigId byte) float64 {
	wl := SigWaveLen(naviSys, sigId, this.Satid)
	if wl == 0 {
		return 0
		//panic(fmt.Sprintf("未知的波长[%s][%d]", GetPrnStrCode(this.Satid, naviSys), sigId))
	}
	return (this.PhRngV[sigId-1] + this.RoughRR) / wl
}

func (this *RtcmRawObsSat) SetSigLockTV(sig byte, v uint16) {
	this.LkTV[sig-1] = v
}

func (this *RtcmRawObsSat) SetSigLLI(sig byte, v byte) {
	this.LLI[sig-1] = v
}

func (this *RtcmRawObsSat) SetCnr(sigId byte, v float32) {
	this.Cnr[sigId-1] = v
}

type RtcmRawMsmObs struct {
	Header RtcmRawObsHeader
	SatObs [64]*RtcmRawObsSat
}

func NewRtcmRawMsmObs() *RtcmRawMsmObs {
	rval := &RtcmRawMsmObs{}
	return rval
}

func (this *RtcmRawMsmObs) IsValid() bool {
	return this.Header.NaviSys != 0
}

func (this *RtcmRawMsmObs) CopyFromMsm(msm RtcmRawMsm) {
	this.Header.CopyInfoFrom(msm.GetHeader())
	msm.WriteToObs(this)
}

func (this *RtcmRawMsmObs) GetMsgTime(reftime time.Time) time.Time {
	return TOW2Time(reftime, uint(this.Header.TOW))
}

func (this *RtcmRawMsmObs) ExistsObs(startPrn byte) bool {
	for satIdx := startPrn - 1; satIdx < byte(len(this.SatObs)); satIdx++ {
		obs := this.SatObs[satIdx]
		if obs != nil && obs.Satid != 0 {
			for sigIdx := U8_0; sigIdx < 32; sigIdx++ {
				if obs.IsValid(sigIdx) {
					return true
				}
			}
		}
	}
	return false
}

func (this *RtcmRawMsmObs) ExtractMsmPackEx(startPrn, maxEndPrn, maxSatN byte, checkfn func(satid uint16, sigid byte) bool) (newsigmask uint32, sigN byte, endPrn byte, satlst []byte) {
	sigN0 := U8_0
	newsigmask0 := newsigmask
	satN0 := U8_0
	for satIdx := startPrn - 1; satIdx < 64; satIdx++ {
		obs := this.SatObs[satIdx]
		if obs != nil && obs.Satid != 0 {
			if maxEndPrn > 0 && obs.Satid > maxEndPrn {
				break
			}

			satFlag := 0

			for sigIdx := U8_0; sigIdx < 32; sigIdx++ {
				if obs.IsValid(sigIdx) && (checkfn(uint16(obs.Satid), sigIdx+1)) {
					satFlag = 1
					if SetSigMaskOn(&newsigmask0, sigIdx+1) {
						sigN0++
					}
				}
			}
			if satFlag == 1 { // 卫星有设定信号
				satN0++

				if satN0*sigN0 > 64 {
					break
				} else {
					newsigmask = newsigmask0
					sigN = sigN0
					endPrn = obs.Satid
					satlst = append(satlst, endPrn)

					if maxSatN > 0 && satN0 >= maxSatN {
						break
					}
				}
			}
		}
	}
	return
}

func (this *RtcmRawMsmObs) ExtractMsmPack(startPrn, maxEndPrn, maxSatN byte, selsiglst []byte, sigfixedlst ...byte) (newsigmask uint32, sigN byte, endPrn byte, satlst []byte) {
	sigN0 := SigsToMask(&newsigmask, sigfixedlst...)
	newsigmask0 := newsigmask
	satN := U8_0
	for satIdx := startPrn - 1; satIdx < 64; satIdx++ {
		obs := this.SatObs[satIdx]
		if obs != nil && obs.Satid != 0 {
			if maxEndPrn > 0 && obs.Satid > maxEndPrn {
				break
			}
			satN++
			for sigIdx := U8_0; sigIdx < 32; sigIdx++ {
				if obs.IsValid(sigIdx) && (len(selsiglst) == 0 || bytes.IndexByte(selsiglst, sigIdx+1) != -1) {
					if SetSigMaskOn(&newsigmask0, sigIdx+1) {
						sigN0++
					}
				}
			}
			if satN*sigN0 > 64 {
				break
			} else {
				newsigmask = newsigmask0
				sigN = sigN0
				endPrn = obs.Satid
				satlst = append(satlst, endPrn)

				if maxSatN > 0 && satN >= maxSatN {
					break
				}
			}
		}
	}
	return
}

func (this *RtcmRawMsmObs) GetEndPrn() (rval byte) {
	siglst := make([]byte, 0, 8)
	satN := 0
	for satIdx := U8_0; satIdx < 64; satIdx++ {
		obs := this.SatObs[satIdx]
		if obs != nil && obs.Satid != 0 {
			satN++
			for sig := U8_0; sig < 32; sig++ {
				if obs.IsValid(sig) {
					if bytes.IndexByte(siglst, sig) == -1 {
						siglst = append(siglst, sig)
					}
				}
			}
			if satN*len(siglst) > 64 {
				break
			} else {
				rval = obs.Satid
			}
		}
	}
	return
}

/*
**

	如果有值则进行改正
*/
func (this *RtcmRawMsmObs) PhaseCheckShift(sig byte, phaseshift float64) {
	for satIdx := 0; satIdx < 64; satIdx++ {
		obs := this.SatObs[satIdx]
		if obs != nil && obs.Satid != 0 {
			if obs.PsRngV[sig-1] != 0 && obs.PhRngV[sig-1] != 0 {
				lli := U8_0
				obs.SetPhPng(this.Header.NaviSys, sig, obs.GetPhPng(this.Header.NaviSys, sig)+phaseshift, &lli, nil, nil)
			}
		}
	}
}

func (this *RtcmRawMsmObs) MergeSig(dstSig, srcSig byte, phaseshift float64, emptySrcSig bool) {
	for satIdx := 0; satIdx < 64; satIdx++ {
		obs := this.SatObs[satIdx]
		if obs != nil && obs.Satid != 0 {
			if obs.PsRngV[dstSig-1] == 0 && obs.PsRngV[srcSig-1] != 0 {
				lli := U8_0
				obs.SetPsPng(dstSig, obs.GetPsPng(srcSig))
				obs.SetPhPng(this.Header.NaviSys, dstSig, obs.GetPhPng(this.Header.NaviSys, srcSig)+phaseshift, &lli, nil, nil)
				obs.SetSigLockTV(dstSig, obs.LkTV[srcSig-1])
				obs.SetCnr(dstSig, obs.Cnr[srcSig-1])
				obs.HalfAmb[dstSig-1] = obs.HalfAmb[srcSig-1]
			}
			if emptySrcSig {
				obs.EmptySigValues(srcSig)
			}
		}
	}
}

/*
**

	统计startPrn到endPrn有多少颗有数据的卫星
	satMask和sigmask如果为0则统计所有的
*/
func (this *RtcmRawMsmObs) ExistsCount(satMask, sigmask uint32, startPrn, endPrn byte) (satN byte) {
	if endPrn <= 0 || endPrn > 64 {
		endPrn = 64
	}

	if satMask == 0 {
		satMask = 0xFFFFFFFF
	}

	if sigmask == 0 {
		sigmask = 0xFFFFFFFF
	}

	for satIdx := startPrn - 1; satIdx <= endPrn-1; satIdx++ {
		satFlag := (satMask >> (63 - satIdx)) & 1
		if satFlag == 1 { // 卫星数据
			obs := this.SatObs[satIdx]
			if obs != nil && obs.Satid != 0 {
				for sigIdx := U8_0; sigIdx < 32; sigIdx++ { // 信号
					sigFlag := (sigmask >> (31 - sigIdx)) & 1
					if sigFlag == 1 { // 信号Mask
						if obs.IsValid(sigIdx) {
							satN++
							break
						}
					}
				}
			}
		}
	}
	return
}

/*
**

	根据卫星信息,信号掩码信息,观测值数据, 获取新的信号掩码信息
*/
func (this *RtcmRawMsmObs) MakeSigMask(satMask uint64, sigmask uint32) (newsigmask uint32) {
	for satIdx := 0; satIdx < 64; satIdx++ {
		satFlag := (satMask >> (63 - satIdx)) & 1
		if satFlag == 1 { // 卫星数据
			obs := this.SatObs[satIdx]
			if obs != nil && obs.Satid != 0 {
				for sigIdx := U8_0; sigIdx < 32; sigIdx++ { // 信号
					sigFlag := (sigmask >> (31 - sigIdx)) & 1
					if sigFlag == 1 { // 信号Mask
						if obs.IsValid(sigIdx) {
							newsigmask |= (1 << (31 - sigIdx))
						}
					}
				}
			}
		}
	}
	return newsigmask
}

/*
*

	获取卫星
*/
func (this *RtcmRawMsmObs) ExtractSats() []byte {
	var rval [64]byte
	j := 0
	for i := 0; i < len(this.SatObs); i++ {
		if this.SatObs[i] != nil {
			id := this.SatObs[i].Satid
			if id != 0 {
				rval[j] = id
				j++
			}
		}
	}
	return rval[:j]
}

func (this *RtcmRawMsmObs) GetSatsN() int {
	j := 0
	for i := 0; i < len(this.SatObs); i++ {
		if this.SatObs[i] != nil {
			id := this.SatObs[i].Satid
			if id != 0 {
				j++
			}
		}
	}
	return j
}

/*
**

	获取信号的Mask
*/
func (this *RtcmRawMsmObs) ExtractSignMask() uint32 {
	var rval uint32 = 0
	for i := 0; i < len(this.SatObs); i++ {
		if this.SatObs[i] != nil {
			id := this.SatObs[i].Satid
			if id != 0 {
				rval = this.SatObs[i].FillSignMask(rval)
			}
		}
	}
	return rval
}

func (this *RtcmRawMsmObs) GetSatObs(satid byte) *RtcmRawObsSat {
	return this.SatObs[satid-1]
}

func (this *RtcmRawMsmObs) Reset() {
	this.Header.NaviSys = 0
	for i := U8_0; i < 64; i++ {
		if this.SatObs[i] != nil {
			this.SatObs[i].Reset()
		}
	}
}

func (this *RtcmRawMsmObs) Clean() {
	this.Header.NaviSys = 0
	for i := U8_0; i < 64; i++ {
		if this.SatObs[i] != nil {
			if this.SatObs[i] != nil {
				this.SatObs[i].Reset()
				rtcm_raw_sat_pool.Put(this.SatObs[i])
				this.SatObs[i] = nil
			}
		}
	}
}

func (this *RtcmRawMsmObs) ResetObs(satid byte) {
	obs := this.SatObs[satid-1]
	if obs != nil {
		obs.Reset()
	}
}

func (this *RtcmRawMsmObs) CheckGetSatObs(satid byte) *RtcmRawObsSat {
	if this.SatObs[satid-1] == nil {
		satobs := rtcm_raw_sat_pool.Get().(*RtcmRawObsSat)
		this.SatObs[satid-1] = satobs
	}
	return this.SatObs[satid-1]
}

/*
*
计算卫星的粗略范围值, 如果为0 说明无法计算, 该卫星不可用
*/
func CalcuRoughRR(yobs *GnssYObs) float64 {
	lst := make(sort.Float64Slice, 0, len(yobs.PsRngV))
	for i := U8_0; i < byte(len(yobs.PsRngV)); i++ {
		if yobs.IsSignalValidIdx(i) {
			lst = append(lst, yobs.PsRngV[i])
		}
	}
	return CalcuRoughRngBySlice(lst)
}

func CalcuRoughRR_V1(v float64) float64 {
	v = math.Round(v*10000) / 10000
	v = v / P2_10_RANGE_MS
	v = math.Round(v*10000) / 10000
	v = math.Round(v)
	return v * P2_10_RANGE_MS
}

func CalcuRoughRR_E(v float64) float64 {
	v = math.Round(v*10000) / 10000
	v = v / P2_10_RANGE_MS
	v = math.Round(v)
	return v * P2_10_RANGE_MS
}

func (this *RtcmRawMsmObs) CopyFromYObs(yobs *GnssYObs, vars *RtcmObsVars) error {
	if yobs.Satid == 0 {
		return nil
	}
	this.Header.NaviSys = yobs.NaviSys

	satObs := this.CheckGetSatObs(yobs.Satid)

	//if (yobs.NaviSys == SYS_GLO && yobs.Satid == 24) {
	//	utils.Log.Debug("debug");
	//}

	if yobs.RoughR != 0 {
		satObs.RoughRR = yobs.RoughR
	} else {
		satObs.RoughRR = 0
		//lst := make(sort.Float64Slice, 0, len(yobs.PsRngV))
		for i := U8_0; i < byte(len(yobs.PsRngV)); i++ {
			v := yobs.PsRngV[i]
			if v != 0 {
				satObs.RoughRR = CalcuRoughRR_V1(v)
				break
			}
		}
	}

	if satObs.RoughRR == 0 { //数据异常
		return fmt.Errorf("计算粗略范围值异常, 伪距列表:[%s]", yobs.GetPsRngString())
	}

	//lst := make(sort.Float64Slice, 0, len(yobs.PsRngV))
	//for i:=U8_0; i<byte(len(yobs.PsRngV)); i++{
	//	if yobs.PsRngV[i] != 0 || yobs.PhRngV[i] != 0{
	//		lst = append(lst, yobs.PsRngV[i]);
	//	}
	//}
	//satObs.RoughRR = CalcuRoughRngBySlice(lst)
	//if satObs.RoughRR == 0{  //数据异常
	//	return fmt.Errorf("计算粗略范围值异常, 伪距列表:[%s]", yobs.GetPsRngString());
	//}

	//if CheckRougRngFail(satObs.RoughRR, lst) > -1 {
	//	utils.Log.WarnTypef("", "[%s]伪距列表与rough rang范围异常, 会有忽略频点的情况:%s", yobs.GetPrnCode(), yobs.GetPsRngString())
	//}

	for i := U8_0; i < byte(len(yobs.PsRngV)); i++ {

		if yobs.PsRngV[i] != 0 || yobs.PhRngV[i] != 0 {
			v0 := yobs.PsRngV[i] - satObs.RoughRR
			if math.Abs(v0) > 292 { // 忽略该频点
				continue
			}
			if satObs.Satid == 0 {
				satObs.Satid = yobs.Satid
			}

			sigid := YObsSigID2RtcmSigID(this.Header.NaviSys, i+1)
			//
			//if satObs.Satid == 5 && sigid == YOBS_GPS_SIGN_L2_2W{
			//  sigid = YOBS_GPS_SIGN_L2_2W;  // debug
			//}
			satObs.SetPsPng(sigid, yobs.PsRngV[i])
			satObs.SetPhPng(this.Header.NaviSys, sigid, yobs.PhRngV[i], &yobs.LLI[i], vars, nil)
			satObs.SetCnr(sigid, yobs.Cnr[i])
			satObs.SetSigLockTV(sigid, uint16(LockTime2Indicator(yobs.LkT[i])))
			satObs.SetSigLLI(sigid, yobs.LLI[i])
		}
	}
	return nil
}

/*
*

	如果超过64要考虑分包
*/
func (this *RtcmRawMsmObs) GetCellN() int {
	sigmask := this.ExtractSignMask()
	sigN := utils.GetBitOnU32(sigmask)
	return int(sigN) * this.GetSatsN()
}

func (this *RtcmRawMsmObs) DelSats(startPrn, endPrn byte) {
	if endPrn < startPrn || endPrn == 0 {
		endPrn = 64
	}
	if startPrn == 0 {
		startPrn = 1
	}

	for i := startPrn; i <= endPrn; i++ {
		this.ResetObs(i)
	}
}

func (this *RtcmRawMsmObs) DelSatsEx(prnlst ...byte) {
	for i := 0; i < len(prnlst); i++ {
		prn := prnlst[i]
		if prn > MAX_BD_PRN_ID || prn < MIN_BD_PRN_ID {
			continue
		}
		this.ResetObs(prn)
	}
}

func (this *RtcmRawMsmObs) ExtractCellMask(satMask uint64, signMask uint32) (cellN byte, cellMask uint64) {
	satN := utils.GetBitOnU64(satMask)
	sigN := utils.GetBitOnU32(signMask)

	sigI := U8_0

	n := satN * sigN
	startBitN := byte(n) - 1
	for satIdx := U8_0; satIdx < 64; satIdx++ {
		satFlag := (satMask >> (63 - satIdx)) & 1
		if satFlag == 1 { // 卫星数据
			obs := this.SatObs[satIdx]
			if obs != nil && obs.Satid != 0 {
				for sigIdx := U8_0; sigIdx < 32; sigIdx++ { // 信号
					sigFlag := (signMask >> (31 - sigIdx)) & 1
					if sigFlag == 1 { // 信号Mask
						if obs.IsValid(sigIdx) { // 卫星的信号有数据
							cellMask |= 1 << (startBitN - sigI)
							cellN++
						} else {
							// 无数据留空
						}
						sigI++
					}
				}
				startBitN -= sigN
				sigI = 0
			}
			//return ;
		}
	}
	return
}

func (this *RtcmRawMsmObs) WriteString(bb *utils.BytesBuilder) {
	for i := 0; i < len(this.SatObs); i++ {
		if this.SatObs[i] == nil || this.SatObs[i].Satid == 0 {
			continue
		}
		this.SatObs[i].Print(this.Header.NaviSys, bb)
	}
}

func (this *RtcmRawMsmObs) WriteDetailWithRaw(bb *utils.BytesBuilder) {
	for i := 0; i < len(this.SatObs); i++ {
		if this.SatObs[i] == nil || this.SatObs[i].Satid == 0 {
			continue
		}
		this.SatObs[i].PrintWithRaw(this.Header.NaviSys, bb)
	}
}

func (this *RtcmRawMsmObs) Print() string {
	var bb utils.BytesBuilder
	for i := 0; i < len(this.SatObs); i++ {
		if this.SatObs[i] == nil || this.SatObs[i].Satid == 0 {
			continue
		}
		this.SatObs[i].Print(this.Header.NaviSys, &bb)
	}
	return bb.String()
}

func (this *RtcmRawMsmObs) Compare(slave *RtcmRawMsmObs, fn func(obs_m, obs_s *GnssObsSign) bool) {
	for i := 0; i < len(this.SatObs); i++ {
		obs0 := this.SatObs[i]
		if obs0 == nil || obs0.Satid == 0 {
			continue
		}
		obs1 := slave.SatObs[i]
		if !obs0.Compare(this.Header.NaviSys, obs1, fn) {
			break
		}
	}
}

func (this *RtcmRawMsmObs) PrintWithRaw() string {
	var bb utils.BytesBuilder
	for i := 0; i < len(this.SatObs); i++ {
		if this.SatObs[i] == nil || this.SatObs[i].Satid == 0 {
			continue
		}
		this.SatObs[i].PrintWithRaw(this.Header.NaviSys, &bb)
	}
	return bb.String()
}

func (this *RtcmRawMsmObs) SetLockTV(v uint16) string {
	var bb utils.BytesBuilder
	for i := 0; i < len(this.SatObs); i++ {
		if this.SatObs[i] == nil || this.SatObs[i].Satid == 0 {
			continue
		}
		this.SatObs[i].SetLockTV(v)
	}
	return bb.String()
}
